/*
 * servo.c
 *
 *  Created on: Nov 19, 2021
 *      Author: zz
 */


#include "servo.h"

extern TIM_HandleTypeDef htim3;

static int last_servo_value[SERVO_NUM];

//  舵机初始化
void servo_init()
{
	//硬件初始化已由cube配置完成
	//初始化定时器T3 内部时钟， PWM1模式输出， 72分频（1us）， ARR=19999（20ms）
	// CH1 对应 OUT7 PB1
	TIM3->CCR1=1500;
}

//  舵机设置 percent [500-2500], 1500为舵机中心位置
void servo_set(uint8_t no, int percent)
{
	if(percent>2500)
		percent=2500;
	if(percent<500)
		percent=500;


	switch(no) {
	case 0:
		TIM3->CCR1= percent;
		break;
	default:
		break;
	}

	last_servo_value[no] = percent;
}

//  舵机位置获取
int  servo_get(uint8_t no)
{
	if(no<0 || no>=SERVO_NUM) {
		return 0xFFFF;
	}
	return last_servo_value[no];
}

void servo_tim_periodelapsed()
{
	HAL_GPIO_WritePin(OUT7_GPIO_Port, OUT7_Pin, GPIO_PIN_SET);
}

void servo_pwm_pulsefinished(TIM_HandleTypeDef *htim)
{
	if(htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
		HAL_GPIO_WritePin(OUT7_GPIO_Port, OUT7_Pin, GPIO_PIN_RESET);
}

